#!/usr/bin/env python3
# -*- coding: UTF-8 -*-

import rospy
import tf
from geometry_msgs.msg import PoseStamped, Point


robot_name = ["tbmn_01", "tbmn_02"]

def pose_publisher():
    while not rospy.is_shutdown():
        for i in range(len(robot_name)):
            pose_publisher = rospy.Publisher('/' + robot_name[i] + '_pose', PoseStamped, queue_size=10)
            rate = rospy.Rate(10)
            tf_listener = tf.TransformListener()
            tf_listener.waitForTransform('/' + robot_name[i] + '/odom', '/' + robot_name[i] + '/base_link', rospy.Time(0), rospy.Duration(60.0))
            trans, _ = tf_listener.lookupTransform('/' + robot_name[i] + '/odom', '/' + robot_name[i] + '/base_link', rospy.Time(0))
            robot_pose = Point(*trans)

            pose = PoseStamped()
            pose.header.frame_id = 'map'
            pose.header.stamp = rospy.Time.now()
            pose.pose.position.x = robot_pose.x
            pose.pose.position.y = robot_pose.y
            pose.pose.position.z = robot_pose.z
            rospy.loginfo("\r\npub " + robot_name[i] + " position \
                        \r\n[x: %.2f, y: %.2f, z: %.2f]" %
                        (robot_pose.x, robot_pose.y, robot_pose.z))

            pose_publisher.publish(pose)
            rate.sleep()


if __name__ == '__main__':
    try:
        rospy.init_node('pose_publisher', anonymous=True)
        pose_publisher()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass
